/*************************************************************************
	> File Name: app.c
	> Author: fht
	> Created Time: 2019年10月17日 星期四 16时12分26秒
 ************************************************************************/

#include<stdio.h>
#include<stdlib.h>
#include<fcntl.h>
#include<unistd.h>
#include<string.h>
#include<math.h>
#include<pthread.h>
#include<log/log.h>
#include "mpu_angle_thread.h"
#include "control_filter.h"

#define DEVICE_NAME "/dev/atkmpu6050"
//---------------------------------------------------------------------------------------------------


extern bear_notify_t gNotify;

static pthread_t gBearpiThread;
static int gThreadExit=0;
static int fd = -1;

struct mpu_cmd{
    u8 reg;
    u8 buf[32];
    u8 len;
};

struct mpu_cmd *mpubuf;

void mysplit(char *src, char **dst, int *num)
{
    char *aa;
    char *val;
    char *regstr=src;
    int i = 0;

    while(1){
        aa = strtok_r(regstr, ",", &val);
        dst[i]=aa;
        ALOGD("%s:i =%d, dst=%s, val = %s\n",__func__,i,dst[i], val);
        *num = ++i;

        if(val != NULL) regstr=val;
        else break;
    }
}


//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
u8 MPU_Init(void)
{
	u8 res;

	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
    usleep(100*1000);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050
	MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
	MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
	MPU_Set_Rate(50);						//设置采样率50Hz
	MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
	res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
    ALOGD("%s: id reg res = 0x%x",__func__, res);
	if(res==MPU_ADDR)//器件ID正确
	{
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
		MPU_Set_Rate(50);						//设置采样率为50Hz
 	}else return 1;
	return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
	u8 data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6;
	return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{
	u8 data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
 	return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值
float MPU_Get_Temperature(void)
{
    u8 buf[2];
    short raw;
	float temp;
	MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
    raw=((u16)buf[0]<<8)|buf[1];
    temp=36.53+((double)raw)/340;
    return temp;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Gyroscope(short *gyros)
{
    u8 buf[6],res;
	res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(res==0)
	{
		*gyros=((u16)buf[0]<<8)|buf[1];
		*(gyros+1)=((u16)buf[2]<<8)|buf[3];
		*(gyros+2)=((u16)buf[4]<<8)|buf[5];
	}
    ALOGD("dylan gyros :%d, %d, %d", gyros[0], gyros[1], gyros[2]);
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Accelerometer(short *acceles)
{
    u8 buf[6],res;
	res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(res==0)
	{
		*acceles=((u16)buf[0]<<8)|buf[1];
		*(acceles+1)=((u16)buf[2]<<8)|buf[3];
		*(acceles+2)=((u16)buf[4]<<8)|buf[5];
	}
    ALOGD("dylan %d, %d, %d", acceles[0], acceles[1], acceles[2]);
    return res;;
}

u8 MPU_Write_Byte(u8 reg,u8 data)
{
    return MPU_Write_Len(MPU_ADDR, reg, 1, &data);
}

u8 MPU_Read_Byte(u8 reg)
{
    u8 regval;
    MPU_Read_Len(MPU_ADDR,reg, 1, &regval);
    return regval;
}

u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    u8 ret = 0;
    //ret = ioctl(fd, IOCSETREGCMD, &reg);
    mpubuf->reg = reg;
    memcpy(mpubuf->buf, buf, len);
    mpubuf->len = len;
    ret = write(fd, mpubuf, sizeof(struct mpu_cmd));
    return ret;
}

u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    u8 ret = 0;
    //ret = ioctl(fd, IOCSETREGCMD, &reg);
    mpubuf->reg = reg;
    mpubuf->len = len;
    ret = read(fd, mpubuf, sizeof(struct mpu_cmd));
    memcpy(buf, mpubuf->buf, len);
    //ALOGD("%s: len = %d, buf = %d", __func__, len, *buf);
    return 0;
}

static void *bearpi_main_thread(void *p){
    float pitch, roll, yaw;
    float template;
    short acceles[3], gyros[3];

    fd = open(DEVICE_NAME, O_RDWR);
    if (fd == -1)
    {
        ALOGD("Failed to open device %s.\n", DEVICE_NAME);
        return NULL;
    }
    ALOGD("Read original value:\n");
    mpubuf = malloc(sizeof(struct mpu_cmd));
    memset(mpubuf, 0, sizeof(struct mpu_cmd));
    MPU_Init();
    usleep(500);
    while(mpu_dmp_init()){
        ALOGD("mpu dmp init continue \n");
        usleep(100*1000);
        if(gThreadExit){
            ALOGD("read dev/mpu exit!!!\n");
            break;
        }
    }

    while (1){
        if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0){
        }
        ALOGD("thread sleep....\n");
        //ioctl(fd, SLEEP);
        ALOGD("thread start\n");
        //ioctl(fd,TIMEOUT_WAKELOCK);

        deflection_angle_notify(pitch, roll, yaw);
        ALOGD("mpu dmp get data , pitch=%f, roll=%f, yaw=%f", pitch, roll, yaw);
        template = MPU_Get_Temperature();
        template_notify(template);
        MPU_Get_Gyroscope(gyros);
        MPU_Get_Accelerometer(acceles);
        original_data_notify(acceles, gyros);
        ALOGD("mpu get template = %f \n", template);
        if(gThreadExit){
            ALOGD("read dev/mpu exit!!!\n");
            break;
        }
        if(roll > 150 || roll < 30){
            ALOGD("open led");
            ioctl(fd, LED_ON);
        }else{
            ALOGD("close led");
            ioctl(fd, LED_OFF);
        }
    }

    close(fd);

    return NULL;
}

void start_process(void)
{
    ALOGD("lemon start process");
    gThreadExit=0;
    pthread_create(&gBearpiThread, NULL, bearpi_main_thread, NULL);
}

void stop_process(void)
{
    ALOGD("lemon stop process");
    gThreadExit=1;
}
